#include "gemo.h"
#include <fstream>
#include <QDebug>

static double computeTriangleArea(Point2d p1, Point2d p2, Point2d p3)
{
// assume the orientation is 1-2-3-1

    return ((p2.x-p1.x)*(p3.y-p1.y) - (p3.x-p1.x)*(p2.y-p1.y))/2.0;
}

double CollisionTime(Point2d p0, Point2d p1, Point2d p2,
                     Vector2d v0, Vector2d v1, Vector2d v2,
                     double &s1, double &s2)
{
    Vector2d v21 = v2 - v1;
    Vector2d p21(p1,p2);
    Vector2d p01(p1,p0);
    Vector2d v01 = v0 - v1;

    double a1 = -p01.x;
    double a2 =  p21.x;
    double a3 =  v01.x;
    double a4 = -v21.x;

    double b1 = -p01.y;
    double b2 =  p21.y;
    double b3 =  v01.y;
    double b4 = -v21.y;

    double A = a2*b4 - a4*b2;
    double B = a2*b3 - a3*b2 + a1*b4 - a4*b1;
    double C = a1*b3 - a3*b1;

    s1 = 0.0;
    s2 = 0.0;

    double time;

    // three points are colinear
    if(fabs(b1*a2-a1*b2)<1e-12){

    }

    if(fabs(A)<1e-16){
        if(fabs(B)>=1e-16){
               s1 = -C / B;
               s2 = -C / B;
        }else{
            if(fabs(C)>=1e-16)
                return -1.0;
        }
    }else{
        if((B*B - 4*A*C)>0.0){
            s1 = (-B + sqrt(B*B - 4*A*C))/A/2.0;
            s2 = (-B - sqrt(B*B - 4*A*C))/A/2.0;
        }else{
            time = -1.0;
            return time;
        }
    }


    time = -1.0;
    if(s1 >=0.0 && s1 <= 1.0 ){
        double tt;
        if(fabs(a3+a4*s1)<1e-12){
            if(fabs(b3+b4*s1)<1e-12){
               if(fabs(s1*b2+b1)<1e-12 && fabs(s1*a2+a1)<1e-12 ){

                    time = 0.0;
                    return time;
                }else{
                    time = -2.0;
                    return time;
                }
            }else{
                tt = (b1+b2*s1)/(b3+b4*s1);
            }
        }else{
            tt = (a1+a2*s1)/(a3+a4*s1);
        }
        if(tt >= 0.0 )
            time = tt;
     }

    if(s2 >=0.0 && s2 <= 1.0 ){
        double tt;
         if(fabs(a3+a4*s1)<1e-12){
            if(fabs(b3+b4*s1)<1e-12){
                if(fabs(s2*b2+b1)<1e-12 && fabs(s2*a2+a1)<1e-12){
                    time = 0.0;
                    return time;
                }else{
                    time = -2.0;
                    return time;
                }
            }else{
                tt = (b1+b2*s2)/(b3+b4*s2);
            }
          }else{
            tt = (a1+a2*s2)/(a3+a4*s2);
          }

        if(tt >= 0.0 )
            time = (time < 0.0 ? tt : std::min(tt, time));
    }

    return time;
}

double MPolygon::SmallestArc()
{
    double chop = 1e20;

    int last = this->PointNo();
    for(int i = 0 ; i < last; i++)
    {
        double xs = this->points[i].x -
                    this->points[(i+1)%last].x;
        double ys = this->points[i].y -
                    this->points[(i+1)%last].y;
        double len = sqrt(xs*xs + ys*ys);

        chop = std::min(chop, len);
    }

    return chop;
}

void MPolygon::remesh(double criteria)
{

        do{
            int last = this->PointNo();
            MPolygon newpoly;
    //        log<<"Before remesh\n";
            for(int i = 0; i< last; i++)
            {
                double xs = this->points[i].x -
                            this->points[(i+1)%last].x;
                double ys = this->points[i].y -
                            this->points[(i+1)%last].y;
                double len = sqrt(xs*xs + ys*ys);

                if(len >= criteria){
                    newpoly.points.push_back(this->points[i]);
//                    NewCard.push_back(shuffle[i]);
                }else{
//                    log<<i<<" : "<<apoly->points[i].x<<" "<<apoly->points[i].y<<" ";
//                    log<<apoly->points[(i+1)%last].x<<" "<<apoly->points[(i+1)%last].y<<" "<<len<<"\n";
                }
            }

            if(this->points.size() == newpoly.points.size())
                break;

//            log<<"remesh is needed\n";


//            log<<"Remesh - new polygon:"<<apoly->PointNo()<<"\n";


            this->points.resize(newpoly.PointNo(), Point2d(0.0, 0.0));
            for(int i = 0; i< this->PointNo(); i++)
            {
                this->points[i].x = newpoly.points[i].x;
                this->points[i].y = newpoly.points[i].y;
    //        log<<i<<" o: "<<apoly->points[i].x<<"  "<<apoly->points[i].y<<"\n";
            }

//            isKeepShuffle = true;
//            shuffle.resize(apoly->PointNo(), 0);
//            for(int i = 0; i < this->PointNo(); i++){

//                shuffle[i] = NewCard[i];
//            }
        }while(1);
}

void MPolygon::subdivision()
{

    std::ofstream out("subout.txt");
    double chop = 1e20;

    int last = this->PointNo();
    for(int i = 0 ; i < last; i++)
    {
        double xs = this->points[i].x -
                    this->points[(i+1)%last].x;
        double ys = this->points[i].y -
                    this->points[(i+1)%last].y;
        double len = sqrt(xs*xs + ys*ys);

        chop = std::min(chop, len);
    }

    out<<chop<<":\n";
    if(chop < 1e-8);
    this->remesh(1e-8);
    chop = this->SmallestArc();
    out<<chop<<":\n";
    MPolygon newp;
    for(int i = 0 ; i < last; i++)
    {
        double xs = this->points[i].x -
                    this->points[(i+1)%last].x;
        double ys = this->points[i].y -
                    this->points[(i+1)%last].y;
        double len = sqrt(xs*xs + ys*ys);

        int part = (int) (len/chop);

        part = std::min(part, 100);

        out<<len<<": "<<part<<"\n";
        for(int j = 0; j < part; j++)
        {
            double inc = (double) j / (double) part;

            double ix = this->points[i].x + inc *
                 (this->points[(i+1)%last].x - this->points[i].x);
            double iy = this->points[i].y + inc *
                 (this->points[(i+1)%last].y - this->points[i].y);

            newp.points.push_back(Point2d(ix, iy));
        }
    }

    out<<"done"<<".\n";
    this->points.resize(newp.PointNo(), Point2d());

    out<<this->PointNo()<<"\n";
    for(int i = 0; i < newp.PointNo(); i++)
    {
        this->points[i].x = newp.points[i].x;
        this->points[i].y = newp.points[i].y;
    }
}


void MPolygon::iArea(std::vector<double> &Ai, int jump)
{
    Ai.resize(this->PointNo(), 0.0);

    int last = PointNo();
    for(int i = 0 ; i < PointNo(); i++)
    {
        double x1 = this->points[(i+last-jump)%last].x;
        double x2 = this->points[(i+last)%last].x;
        double x3 = this->points[(i+last+jump)%last].x;

        double y1 = this->points[(i+last-jump)%last].y;
        double y2 = this->points[(i+last)%last].y;
        double y3 = this->points[(i+last+jump)%last].y;

        Ai[i] = ((x2-x1)*(y3-y1) - (x3-x1)*(y2-y1))/2.0;
    }
}

void MPolygon::reorder(double criteria, std::vector<int> &Token)
{
    std::vector<double> Ar;

    Token.resize(PointNo(), 0);
    for(int i = 0; i < PointNo(); i++ )
        Token[i] = i;
    std::ofstream log("order_log.txt");


    do{

        int last = PointNo();
        this->iArea(Ar, 1);

        for(int i = 0; i < last; i ++)
        {

            if(fabs(Ar[i])<criteria)
            {// three points are colinear

                double x1, x2, x3;
                double y1, y2, y3;

                x1 = this->points[(i+last-1)%last].x;
                y1 = this->points[(i+last-1)%last].y;

                x2 = this->points[(i+last)%last].x;
                y2 = this->points[(i+last)%last].y;

                x3 = this->points[(i+last+1)%last].x;
                y3 = this->points[(i+last+1)%last].y;

                double seg12 = sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2));
                double seg23 = sqrt((x2-x3)*(x2-x3)+(y2-y3)*(y2-y3));
                double seg13 = sqrt((x1-x3)*(x1-x3)+(y1-y3)*(y1-y3));


                if(seg13<seg23 || seg13<seg12)
                {// i+1 befor i-1

                    this->points.erase(this->points.begin()+i);
                    Token.erase(Token.begin() + i);
                    break;
                }

             }

          }

          if(last == this->points.size())
            break;

    }while(1);


}

Vector2d MPolygon::Segment(int idx){
    int md = PointNo();
    Point2d p1(this->points[idx].x, this->points[idx].y);
    Point2d p2(this->points[(idx+1)%md].x, this->points[(idx+1)%md].y);

    return Vector2d(p1, p2);
}

double MPolygon::Area(){
    double sum = 0.0;
    int last = PointNo();
    for(int i = 0; i < last; i++)
    {
        sum += points[i].x*points[(i+1)%last].y -
               points[i].y*points[(i+1)%last].x;
    }

    sum /= 2.0;
    return sum;
}

void splitPolygon(MPolygon TPoly,
                  std::vector<int> TouchPoints,
                  std::vector<int> Segments,
                  std::vector<MPolygon> &Polygons)
{
    
    if(TouchPoints.size()<1) return;
    if(TouchPoints.size() != Segments.size()) return;
    if(TPoly.PointNo()<1) return;

    std::vector<MPolygon> newPolygons;
    int last = TPoly.PointNo();

    int _point = TouchPoints[0];
    int _segment = Segments[0];

    int firstSize = 0;
    int secondSize = 0;
    std::vector<int> PointsInLoop1;
    std::vector<int> PointsInLoop2;
    std::vector<int> SegmentsInLoop1;
    std::vector<int> SegmentsInLoop2;
    std::vector<MPolygon> APolygons;
    std::vector<MPolygon> BPolygons;

    secondSize = (_segment - _point + last) % last + 1;
    firstSize = last + 1 - secondSize;

    for(int j= 1; j < TouchPoints.size(); j++){
        if((TouchPoints[j] - _point + last) % last < secondSize){
            SegmentsInLoop2.push_back((Segments[j] - _point + last) % last );
            PointsInLoop2.push_back((TouchPoints[j] - _point + last) % last );
        }
        if((TouchPoints[j] - _segment - 1 + last) % last < firstSize){
            SegmentsInLoop1.push_back((Segments[j] - _segment - 1 + last) % last);
            PointsInLoop1.push_back((TouchPoints[j] - _segment - 1 + last) % last);
        }
    }

    MPolygon newpoly1, newpoly2;
    for(int pt = 0; pt < firstSize; pt++)
    {
        int old = (pt + _segment+1) % last;
        newpoly1.points.push_back(TPoly.points[old]);;
    }

    for(int pt = 0; pt < secondSize; pt++)
    {
        int old = (pt + _point) % last;
        newpoly2.points.push_back(TPoly.points[old]);
    }
    

    if(PointsInLoop1.size()<1){
        newPolygons.push_back(newpoly1);
    }else{
        splitPolygon(newpoly1,
                     PointsInLoop1,
                     SegmentsInLoop1,
                     APolygons);
    }

    if(PointsInLoop2.size()<1){
        newPolygons.push_back(newpoly2);
    }else{
        splitPolygon(newpoly2,
                     PointsInLoop2,
                     SegmentsInLoop2,
                     BPolygons);
    }

    for(int j= 0; j < APolygons.size(); j++)
        newPolygons.push_back(APolygons[j]);
    for(int j= 0; j < BPolygons.size(); j++)
        newPolygons.push_back(BPolygons[j]);


    Polygons.clear();
    for(int j= 0; j < newPolygons.size(); j++)
        Polygons.push_back(newPolygons[j]);
    return;


}

